/*
 * moving_objetcs.cpp
 *
 *  Created on: Apr 24, 2013
 *      Author: filipe
 */
#include <math.h>
#include <stdio.h>
#include "opencv2/core/core.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/nonfree/nonfree.hpp"

using namespace cv;

int main(int argc, char** argv)
{
	if (argc < 3)
	{
		printf("Use %s <img1> <img2>\n", argv[0]);
		exit(-1);
	}

	Mat colored_img = imread(argv[1], CV_LOAD_IMAGE_COLOR);

    Mat img1 = imread(argv[1], CV_LOAD_IMAGE_GRAYSCALE);
    Mat img2 = imread(argv[2], CV_LOAD_IMAGE_GRAYSCALE);


    if(img1.empty() || img2.empty())
    {
        printf("Can't read one of the images\n");
        return -1;
    }

    // detecting keypoints
    SurfFeatureDetector detector(400);
    vector<KeyPoint> keypoints1, keypoints2;
    detector.detect(img1, keypoints1);
    detector.detect(img2, keypoints2);

    // computing descriptors
    SurfDescriptorExtractor extractor;
    Mat descriptors1, descriptors2;
    extractor.compute(img1, keypoints1, descriptors1);
    extractor.compute(img2, keypoints2, descriptors2);

    // matching descriptors
    BFMatcher matcher(NORM_L2);
    vector<DMatch> matches;
    matcher.match(descriptors1, descriptors2, matches);

    // drawing the results
    namedWindow("matches", 1);
    Mat img_matches;
    // drawMatches(img1, keypoints1, img2, keypoints2, matches, img_matches);
    // imshow("matches", img_matches);
    // waitKey(0);

    int found;
    vector<float> objects;
    vector<Scalar> colors;

    double max = 0.0, min = DBL_MAX;

    for (int i = 0; i < matches.size(); i++)
    {
    	float dist = matches[i].distance;
    	found = 0;

    	if (dist < min)
    		min = dist;

    	if (dist > max)
    		max = dist;

    	for (int j = 0; j < objects.size(); j++)
    	{
    		if (fabs(objects[j] - dist) < (15.0 / (((double) img1.cols) / 2.0)))
    		{
    			objects[j] = (objects[j] + dist) / 2.0;

    			circle(colored_img, keypoints1[matches[i].trainIdx].pt, 1, colors[j], 3, 1, 0);
    			keypoints2[matches[i].queryIdx].pt;

    			found = 1;
    		}
    	}

    	if (!found)
    	{
    		objects.push_back(dist);

    		double x = (dist - min) / (max - min);

    		int rand_r = (int) ((x) * 255);
    		int rand_g = (int) ((x) * 255);
    		int rand_b = (int) ((x) * 255);

    		colors.push_back(Scalar(rand_b, rand_g, rand_r, 255));
    	}
    }

    printf("max: %lf min: %lf\n", max, min);

	imshow("colored_img", colored_img);
	waitKey(0);

    printf("num objects found: %ld\n", objects.size());
    return 0;
}




